import Plugins.Map.utils.math_helpers as math_helpers
from ETS2LA.Utils import settings
import Plugins.Map.classes as c
import Plugins.Map.data as data
import numpy as np
import logging
import math
import time


class RouteItem:
    item: list[c.Prefab | c.Road]
    _lane_index: int = 0
    lane_points: list[c.Position]

    @property
    def lane_index(self) -> int:
        return self._lane_index

    @lane_index.setter
    def lane_index(self, value: int):
        try:
            if isinstance(self.item, c.Road):
                self.lane_points = self.item.lanes[value].points
            elif isinstance(self.item, c.Prefab):
                self.lane_points = self.item.nav_routes[value].points
            else:
                print("Invalid item type")
                print(type(self.item))
            self._lane_index = value
        except Exception:
            # logging.exception(f"Something tried to set an [red]invalid lane index of {value}[/red] when [dim]RouteItem[/dim] only has {len(self.item.lanes)} lanes.")
            pass


class RouteSection:
    items: list[RouteItem]

    lane_change_start: c.Position
    lane_change_factor: float = 0
    is_lane_changing: bool = False
    lane_change_distance: float = 0
    lane_change_points: list[c.Position] = []

    is_ended: bool = False
    invert: bool = False
    path_index: int = 0

    last_actual_points: list[c.Position] = []
    last_lane_points: list[c.Position] = []
    lane_points: list[c.Position] = []

    force_lane_change: bool = False
    skip_indicate_state: bool = False

    _lane_index: int = 0
    _last_lane_index: int = 0
    _target_lanes: list[int] = []
    _lane_change_progress: float = 0.0
    _start_at_truck: bool = True

    _start_node: c.Node = None
    _end_node: c.Node = None

    _first_set_done: bool = False
    """Used to override some checks in the lane_index setter until the function is run once."""

    @property
    def start_node(self) -> c.Node:
        if self._start_node is not None:
            return self._start_node
        if isinstance(self.items[0].item, c.Prefab):
            # Find the index of the node that matches the first curve
            nav_route = self.items[0].item.prefab_description.nav_routes[
                self.lane_index
            ]
            first_curve = nav_route.curves[0]
            index = self.items[0].item.prefab_description.nav_curves.index(first_curve)
            node_index = 0
            for i, node in enumerate(self.items[0].item.prefab_description.nodes):
                if index in node.output_lanes:
                    node_index = i
                    break

            self._start_node = data.map.get_node_by_uid(
                self.items[0].item.node_uids[node_index]
            )
            return self._start_node

        self._start_node = data.map.get_node_by_uid(self.items[0].item.start_node_uid)
        return self._start_node

    @property
    def end_node(self) -> c.Node:
        if self._end_node is not None:
            return self._end_node
        if isinstance(self.items[0].item, c.Prefab):
            # Find the index of the node that matches the last curve
            nav_route = self.items[0].item.prefab_description.nav_routes[
                self.lane_index
            ]
            last_curve = nav_route.curves[-1]
            index = self.items[0].item.prefab_description.nav_curves.index(last_curve)
            node_index = len(self.items[0].item.prefab_description.nodes) - 1
            for i, node in enumerate(self.items[0].item.prefab_description.nodes):
                if index in node.input_lanes:
                    node_index = i
                    break

            self._end_node = data.map.get_node_by_uid(
                self.items[0].item.node_uids[node_index]
            )
            return self._end_node

        self._end_node = data.map.get_node_by_uid(self.items[-1].item.end_node_uid)
        return data.map.get_node_by_uid(self.items[-1].item.end_node_uid)

    @property
    def lane_index(self) -> int:
        return self._lane_index

    @lane_index.setter
    def lane_index(self, value: int):
        if value == self._lane_index and self._first_set_done:
            return
        elif not self._first_set_done:
            self._first_set_done = True

        if isinstance(self.items[0].item, c.Prefab):
            self._last_lane_index = self._lane_index
            self._lane_index = value
            self.lane_points = self.items[0].item.nav_routes[self.lane_index].points
            if self.invert:
                self.lane_points = self.lane_points[::-1]
            return

        if len(self.items[0].item.lanes) == 1:
            value = 0

        if value > len(self.items[0].item.lanes) - 1 or value < 0:
            logging.warning(
                f"Something tried to set an [red]invalid lane index of {value}[/red] when [dim]RouteSection[/dim] only has {len(self.items[0].item.lanes)} lanes."
            )
            return

        if self.is_lane_changing:
            logging.warning(
                "Something tried to change the lane index while the route section is still lane changing."
            )
            return

        self.last_lane_points = self.lane_points.copy() if self.lane_points else []
        new_lane_points: list[c.Position] = []
        for item in self.items:
            item.lane_index = value
            new_lane_points += item.lane_points

        new_lane_points = new_lane_points if not self.invert else new_lane_points[::-1]

        if self.lane_points:
            lanes_to_move_over = abs(value - self.lane_index)
            lane_change_distance = (
                self.get_planned_lane_change_distance(lane_count=lanes_to_move_over) * 2
            )  # * 2 for the initial static area
            if self.skip_indicate_state:
                lane_change_distance /= 2

            dist_left = 0
            if not self.is_in_bounds(
                c.Position(data.truck_x, data.truck_y, data.truck_z)
            ):
                dist_left = self.distance_left(from_index=0)
            else:
                dist_left = self.distance_left()

            if dist_left < lane_change_distance:
                if self.force_lane_change:
                    self.skip_indicate_state = True
                    lane_change_distance = dist_left
                else:
                    logging.warning(
                        f"Something tried to do a lane change requiring [dim]{lane_change_distance:.0f}m[/dim], but only [dim]{dist_left:.0f}m[/dim] is left."
                    )
                    return

            if self._start_at_truck:
                self.lane_change_start = c.Position(
                    data.truck_x, data.truck_y, data.truck_z
                )
            else:
                start = self.last_lane_points[0]
                end = self.last_lane_points[-1]
                s_distance = math_helpers.DistanceBetweenPoints(
                    start.tuple(), (data.truck_x, data.truck_y, data.truck_z)
                )
                e_distance = math_helpers.DistanceBetweenPoints(
                    end.tuple(), (data.truck_x, data.truck_y, data.truck_z)
                )
                if s_distance < e_distance:
                    self.lane_change_start = c.Position(start.x, start.y, start.z)
                else:
                    self.lane_change_start = c.Position(end.x, end.y, end.z)

            self.lane_change_distance = lane_change_distance
            self.lane_change_points = self._calculate_lane_change_points(
                self.last_lane_points, new_lane_points
            )

            if self.lane_change_points:
                self.is_lane_changing = True
                self._lane_change_progress = 0.0

        self.lane_points = new_lane_points
        self._last_lane_index = self._lane_index
        self._lane_index = value
        self.skip_indicate_state = False

    def _calculate_lane_change_points(
        self, start_points: list[c.Position], end_points: list[c.Position]
    ):
        """Pre-calculate lane change points between start_points and end_points"""
        if not start_points or not end_points:
            return []

        closest_index = 0
        closest_distance = 0
        for i, point in enumerate(start_points):
            dist = math_helpers.DistanceBetweenPoints(
                point.tuple(), self.lane_change_start.tuple()
            )
            if closest_distance == 0 or dist < closest_distance:
                closest_distance = dist
                closest_index = i

            if closest_distance < 2:
                break  # Close enough, no need to check further

            if i - closest_index > 20 and closest_distance - dist > 20:
                break  # No need to check further, we're getting farther away again

        start_points = start_points[closest_index:]
        end_points = end_points[closest_index:]

        # Ensure both lists are the same length
        while len(start_points) > len(end_points):
            start_points.pop()
        while len(end_points) > len(start_points):
            end_points.pop()

        if not start_points or not end_points:
            return []

        lane_change_points = []
        end_index = 0
        factors = []
        for i in range(len(start_points)):
            try:
                start_point = start_points[i]
                end_point = end_points[i]
                middle_point = math_helpers.TupleMiddle(
                    start_point.tuple(), end_point.tuple()
                )
                distance = math_helpers.DistanceBetweenPoints(
                    middle_point, self.lane_change_start.tuple()
                )

                if distance >= self.lane_change_distance - 1:
                    # After this point, we'll just use the destination lane points
                    end_index = i
                    break

                # Calculate the smoothed transition factor
                s = distance / self.lane_change_distance
                if not self.skip_indicate_state:
                    if s < 0.5:
                        s = 0
                    else:
                        s = (s - 0.5) * 2

                factor = math_helpers.InOut(s)
                factors.append(factor)
            except Exception:
                continue

        # Lerp between the start and end points using the calculated factors
        # and add the points to the lane_change_points list
        for i in range(len(start_points)):
            if i < end_index:
                new_tuple = math_helpers.LerpTuple(
                    start_points[i].tuple(), end_points[i].tuple(), factors[i]
                )
                lane_change_points.append(
                    c.Position(new_tuple[0], new_tuple[1], new_tuple[2])
                )
            else:
                lane_change_points.append(end_points[i])

        return lane_change_points

    @property
    def target_lanes(self) -> list[int]:
        return self._target_lanes

    @target_lanes.setter
    def target_lanes(self, value: list[int]):
        if self.is_lane_changing:
            return
        if isinstance(self.items[0].item, c.Prefab):
            return
        if self.lane_points == []:
            return
        if value == []:
            return

        closest = min(value, key=lambda x: abs(x - self.lane_index))
        if closest == self.lane_index:
            return
        if closest > len(self.items[0].item.lanes) - 1 or closest < 0:
            logging.warning(
                f"Something tried to set an [red]invalid target lane index of {closest}[/red] when [dim]RouteSection[/dim] only has {len(self.items[0].item.lanes)} lanes."
            )
            return

        # Check if the other lane is on the wrong side of the road
        cur_lane = self.items[0].item.lanes[self.lane_index]
        other_lane = self.items[0].item.lanes[closest]
        if cur_lane.side != other_lane.side:
            logging.warning(
                f"Something tried to change lanes to [dim]{closest}[/dim] but the lane is on the wrong side of the road."
            )
            return

        # Call lane_index setter to update the lane index
        self.force_lane_change = True
        if not self.is_in_bounds(c.Position(data.truck_x, data.truck_y, data.truck_z)):
            self._start_at_truck = False
            self.lane_index = closest
            self._start_at_truck = True
        else:
            self.lane_index = closest
        self.force_lane_change = False

    def distance_left(self, from_index=None) -> float:
        points = self.lane_points
        # equally space up to 10 points
        # 10 provides enough resolution for distance calculations
        if len(points) > 10:
            points = np.array(points)
            indices = np.linspace(0, len(points) - 1, 10).astype(int)
            points = points[indices].tolist()

        distance = 0
        if from_index is not None:
            for i in range(from_index, len(points) - 1):
                distance += math_helpers.DistanceBetweenPoints(
                    points[i].tuple(),
                    points[i + 1].tuple(),
                )
        else:
            closest_index = 0
            closest_distance = 0
            for i, point in enumerate(points):
                dist = math_helpers.DistanceBetweenPoints(
                    point.tuple(), (data.truck_x, data.truck_y, data.truck_z)
                )
                if closest_distance == 0 or dist < closest_distance:
                    closest_distance = dist
                    closest_index = i

            distance += closest_distance
            points = points[closest_index:]
            for i in range(len(points) - 1):
                distance += math_helpers.DistanceBetweenPoints(
                    points[i].tuple(),
                    points[i + 1].tuple(),
                )

        return distance

    def get_planned_lane_change_distance(self, lane_count=1) -> float:
        speed_kph = data.truck_speed * 3.6
        lane_change_distance = speed_kph * data.lane_change_distance_per_kph
        if lane_change_distance < data.minimum_lane_change_distance:
            lane_change_distance = data.minimum_lane_change_distance
        return lane_change_distance * lane_count

    def discard_points_behind(
        self, points: list[c.Position], prune_far=True
    ) -> list[c.Position]:
        forward_vector = [
            -math.sin(data.truck_rotation),
            -math.cos(data.truck_rotation),
        ]
        distances = []
        points_in_front = []
        for i, point in enumerate(points):
            distance = math_helpers.DistanceBetweenPoints(
                point.tuple(), (data.truck_x, data.truck_y, data.truck_z)
            )
            if distance > 150 and i < len(points) - 4 and prune_far:
                continue

            point_forward_vector = [point.x - data.truck_x, point.z - data.truck_z]

            # Clip value to ensure it's within [-1, 1] before passing to np.arccos
            dot_product = np.dot(forward_vector, point_forward_vector)
            norms_product = np.linalg.norm(forward_vector) * np.linalg.norm(
                point_forward_vector
            )

            if norms_product == 0:
                angle = 0.0
            else:
                value = dot_product / norms_product
                value = np.clip(value, -1.0, 1.0)
                angle = np.arccos(value)

            angle = math.degrees(angle)
            if angle > 90 or angle < -90:
                continue

            distances.append(distance)
            points_in_front.append(point)

        if points_in_front == [] or distances == []:
            return []

        closest_point_index = distances.index(min(distances))
        points_in_front = points_in_front[closest_point_index:]

        return points_in_front

    def is_in_bounds(self, point: c.Position, offset: int = -5) -> bool:
        temp_y = point.y
        point.y = point.z
        point.z = temp_y
        for item in self.items:
            if isinstance(item.item, c.Road):
                if item.item.bounding_box.is_in(point, offset=offset):
                    return True
        return False

    def reset_indicators(self):
        if data.enabled and self.is_in_bounds(
            c.Position(data.truck_x, data.truck_y, data.truck_z)
        ):
            if data.truck_indicating_left:
                data.controller.lblinker = True
                time.sleep(1 / 20)
                data.controller.lblinker = False
                time.sleep(1 / 20)
            elif data.truck_indicating_right:
                data.controller.rblinker = True
                time.sleep(1 / 20)
                data.controller.rblinker = False
                time.sleep(1 / 20)

    def cancel_lane_change(self):
        self.is_lane_changing = False
        self._lane_change_progress = 0
        self.lane_points = self.last_lane_points
        self.reset_indicators()

    def indicate_right(self):
        if data.enabled:
            if data.truck_indicating_left:
                self.cancel_lane_change()
                return

            if not data.truck_indicating_right:
                data.controller.rblinker = True
                time.sleep(1 / 20)
                data.controller.rblinker = False
                time.sleep(1 / 20)

    def indicate_left(self):
        if data.enabled:
            if data.truck_indicating_right:
                self.cancel_lane_change()
                return

            if not data.truck_indicating_left:
                data.controller.lblinker = True
                time.sleep(1 / 20)
                data.controller.lblinker = False
                time.sleep(1 / 20)

    def get_points(self):
        # Check the setting so the indicators work correctly in UK for example
        self._traffic_side = settings.Get("Map", "traffic_side", "")

        # If not lane changing, return the normal lane points
        if not self.is_lane_changing or isinstance(self.items[0].item, c.Prefab):
            current_lane_points = self.discard_points_behind(self.lane_points)
            if len(current_lane_points) < 2:
                self.is_ended = True
                if isinstance(self.items[0].item, c.Road):
                    self.reset_indicators()
            else:
                self.is_ended = False

            self.last_actual_points = current_lane_points
            return current_lane_points

        if self.lane_change_distance <= 0:
            planned = self.get_planned_lane_change_distance()
            self.lane_change_distance = (
                planned if self.distance_left() > planned else self.distance_left()
            )

        # Update lane change progress based on distance traveled
        if self.is_in_bounds(c.Position(data.truck_x, data.truck_y, data.truck_z)):
            self._lane_change_progress = (
                math_helpers.DistanceBetweenPoints(
                    self.lane_change_start.tuple(),
                    (data.truck_x, data.truck_y, data.truck_z),
                )
                / self.lane_change_distance
            )
            self.lane_change_factor = math_helpers.InOut(self._lane_change_progress)

        if self._traffic_side == "Automatic":
            try:
                x = data.truck_x
                y = data.truck_z

                calais = [-31100, -5500]
                is_uk = x < calais[0] and y < calais[1]

                if is_uk:
                    uk_factor = 0.75
                    x = (x + calais[0] / 2) * uk_factor
                    y = (y + calais[1] / 2) * uk_factor

                left_hand = is_uk

            except Exception:
                left_hand = False
        else:
            left_hand = self._traffic_side == "Left"

        # Set traffic side
        self._traffic_side = "Left" if left_hand else "Right"

        # Lane-change logic
        if self.is_lane_changing and self._lane_change_progress > 0:
            side = self.items[0].item.lanes[self.lane_index].side
            if left_hand:  # reverse for left-hand traffic
                side = "left" if side == "right" else "right"

            diff = self._last_lane_index - self.lane_index

            if (side == "left" and diff > 0 and not data.truck_indicating_right) or (
                side == "right" and diff < 0 and not data.truck_indicating_right
            ):
                self.indicate_right()
            elif (side == "left" and diff < 0 and not data.truck_indicating_left) or (
                side == "right" and diff > 0 and not data.truck_indicating_left
            ):
                self.indicate_left()

        if not self.is_lane_changing:  # got cancelled in the indicate functions
            return self.discard_points_behind(self.lane_points)

        # Check if lane change is complete
        if self._lane_change_progress > 0.98:
            self.is_lane_changing = False
            # Turn off blinkers after lane change
            self.reset_indicators()

            self.lane_change_factor = 0
            self._lane_change_progress = 0
            tmp = self.discard_points_behind(self.lane_points)
            self.last_actual_points = tmp
            return tmp

        # Get relevant lane change points based on current position
        current_lane_points = self.discard_points_behind(self.lane_change_points)

        if len(current_lane_points) < 2:
            self.is_ended = True
            if isinstance(self.items[0].item, c.Road):
                self.reset_indicators()

            # Fallback to last points if we don't have enough lane change points
            if self.last_actual_points:
                return self.last_actual_points
            return []
        else:
            self.is_ended = False

        self.last_actual_points = current_lane_points
        return current_lane_points

    def __str__(self):
        return f"RouteSection: {self.start_node.uid} ({math_helpers.DistanceBetweenPoints((self.start_node.x, self.start_node.y), (data.truck_x, data.truck_z))}) -> {self.end_node.uid} ({math_helpers.DistanceBetweenPoints((self.end_node.x, self.end_node.y), (data.truck_x, data.truck_z))})\n\
                Lane index: {self.lane_index}\n\
                Distance left: {self.distance_left():.0f}m\n\
                Lane changing: {self.is_lane_changing}\n\
                Is ended: {self.is_ended}\n\
                Type: {type(self.items[0].item)}"

    def __eq__(self, value: object) -> bool:
        if not isinstance(value, RouteSection):
            return False
        return (
            self.start_node.uid == value.start_node.uid
            and self.end_node.uid == value.end_node.uid
            and self.lane_index == value.lane_index
        )

    def information_json(self):
        return {
            "uids": [item.item.uid for item in self.items],
            "lane_index": self.lane_index,
            "type": type(self.items[0].item).__name__,
            "is_ended": self.is_ended,
            "is_lane_changing": self.is_lane_changing,
            "lane_points": [point.tuple() for point in self.lane_points]
            if self.is_lane_changing
            else [],
            "last_lane_points": [point.tuple() for point in self.last_lane_points]
            if self.is_lane_changing
            else [],
        }
